"""
版本V0.3
20221109beta
基本实现功能：电机驱动测试、参数收集、带宽测试、神经网络模型应用
"""
import os
import random
import time
import tkinter.messagebox as MSG
from math import *
from threading import Thread
from tkinter import *
from tkinter import filedialog
from tkinter.ttk import Combobox
from tkinter import ttk

import numpy
import numpy as np
import openpyxl
import serial.tools.list_ports

import DrEmpower_ascii as dr_asc
import DrEmpower_can as dr_can
import DrEmpower_uart as dr_uart
import matplotlib.pyplot as plt
import NN_BP

TEST_TIMES = 100  # 测试次数

root = Tk()
root.title('参数测试 V0.3')  # 界面标题
root.geometry('800x280')  # 界面大小

Com_flag = False  # CAN连接状态
test_flag = False  # 测试状态
aut_flag = False   # 自动测试状态
Type_LL = False  # 是否为低成本
vel_gain = 999999  # 速度增益
vel_inte = 999999  # 速度积分
pos_gain = 999999  # 位置增益
cur_band = 999999  # 电流带宽

frequency_HZ_ST = 100  # ST方案轨迹跟踪模式发送频率
frequency_HZ_LL = 50  # 低成本方案轨迹跟踪模式发送频率
frequency_HZ = frequency_HZ_ST  # 轨迹跟踪模式发送频率

err_data_cur = []  # 电流测试数据集
err_data_speed = []  # 速度测试数据集
err_data_pos = []  # 位置测试数据集

last_data_cur = []  # 上次电流测试数据分析结果
last_data_speed = []  # 上次速度测试数据分析结果
last_data_pos = []  # 上次位置测试数据分析结果

change_mode = 0  # 当次更改的参数类型
last_change_mode = 0  # 上次更改的参数类型

temp_limit = 75  # 电机测试的温度上限
Zero_error = False

excel_en = IntVar()


# 获取当次变更的参数类型
def get_change():
    change = 0
    if change_mode == 0:
        return change
    if change_mode == 1:
        change = ['A1']
    if change_mode == 2:
        change = ['A0']
    if change_mode == 3:
        change = ['B1']
    if change_mode == 4:
        change = ['B0']
    if change_mode == 5:
        change = ['C1']
    if change_mode == 6:
        change = ['C0']
    if change_mode == 7:
        change = ['D1']
    if change_mode == 8:
        change = ['D0']
    return change + [str(get_step())]


# 处理数据集,输出最大值,最小值,均值,方差值
def data_processing(datas):
    data_max = np.max(datas)
    data_min = np.min(datas)
    # speed = get_speed()
    # if speed != 0:
    # 	data_mean = np.mean(datas) / speed
    # else:
    data_mean = np.mean(datas)
    data_var = np.var(datas)
    # print(f'最大值：{data_max}最小值：{data_min}')
    # print(f'平均值：{data_mean}方差：{data_var}')
    return [data_max, data_min, data_mean, data_var]


# CAN读取测试数据
def read_data():
    global test_flag, Zero_error

    def m_abs(a):
        if a < 0:
            return -a
        return a

    motor_id = get_id()
    if motor_id == '':
        return

    err_data_cur.append(m_abs(float(dr_can.read_property(motor_id, 'axis0.motor.current_control.Iq_setpoint')) -
                              float(dr_can.read_property(motor_id, 'axis0.motor.current_control.Id_measured'))))  # 电流差值
    err_data_speed.append(m_abs(float(dr_can.read_property(motor_id, 'axis0.controller.vel_setpoint')) -
                                float(dr_can.read_property(motor_id, 'axis0.encoder.vel_estimate'))))  # 速度差值
    err_data_pos.append(m_abs(float(dr_can.read_property(motor_id, 'axis0.controller.pos_setpoint')) -
                              float(dr_can.read_property(motor_id, 'axis0.encoder.pos_estimate'))))  # 位置差值
    dvar = np.var(err_data_speed)
    # print(dvar)
    if dvar > 0.35:
        test_flag = False
        Zero_error = True
        dr_can.set_mode(motor_id, 1)
        MSG.showerror('错误', '电机异常退出测试，请注意零点位置重新设置零点')


# USB读取测试数据(单独线程)
def USBread_data():
    def m_abs(a):
        if a < 0:
            return -a
        return a

    def read():
        global test_flag, Zero_error
        motor_id = get_id()
        read_num = 0
        err_num = 0
        if motor_id == '':
            return
        while True:
            if not test_flag:
                print(f"成功读取{read_num}次 失败{err_num}次")
                return
            try:
                if Type_LL:
                    # 电流差值
                    err_data_cur.append(
                        float(dr_uart.read_property(motor_id, 'axis0.motor.current_control.Iq_setpoint')) -
                        float(dr_uart.read_property(motor_id, 'axis0.motor.current_control.Iq_measured')))
                    # 速度差值
                    err_data_speed.append(float(dr_uart.read_property(motor_id, 'axis0.controller.vel_setpoint')) -
                                          float(dr_uart.read_property(motor_id, 'axis0.encoder.vel_estimate')))
                    # 位置差值
                    err_data_pos.append(float(dr_uart.read_property(motor_id, 'axis0.controller.pos_setpoint')) -
                                        float(dr_uart.read_property(motor_id, 'axis0.encoder.pos_estimate')))
                else:
                    # 电流差值
                    err_data_cur.append(
                        float(dr_asc.read_property(motor_id, 'axis0.motor.current_control.Iq_setpoint')) -
                        float(dr_asc.read_property(motor_id, 'axis0.motor.current_control.Iq_measured')))
                    # 速度差值
                    err_data_speed.append(float(dr_asc.read_property(motor_id, 'axis0.controller.vel_setpoint')) -
                                          float(dr_asc.read_property(motor_id, 'axis0.encoder.vel_estimate')))
                    # 位置差值
                    err_data_pos.append(float(dr_asc.read_property(motor_id, 'axis0.controller.pos_setpoint')) -
                                        float(dr_asc.read_property(motor_id, 'axis0.encoder.pos_estimate')))
                dvar = np.var(err_data_speed)
                # print(dvar)
                if dvar > 0.35:
                    test_flag = False
                    Zero_error = True
                    MSG.showerror('错误', '电机异常退出测试，请注意零点位置重新设置零点')
                read_num = read_num + 1
                time.sleep(0.01)
            except Exception:
                err_num = err_num + 1
                time.sleep(0.01)

    Thread(target=read).start()


# 扭矩测试模式(准备弃用)
def test_cur(mid, test, cur):
    global test_flag, save_flag
    global last_data_speed, last_data_cur, last_change_mode

    dr_can.set_torque(id_num=mid, torque=cur)
    if test != 0:
        err_data_cur.clear()
        err_data_speed.clear()
        err_data_pos.clear()
        for i in range(TEST_TIMES):
            if not test_flag:
                MSG.showinfo('提示', '测试已退出')
                return
            read_data()

        dr_can.set_mode(mid, 1)
        test_flag = False
        button_setZero.configure(text='设置零点')
        MSG.showinfo('提示', '测试完成')

    if save_flag and excel_en.get() == 1:
        button_save_command()
        save_flag = False
    elif test != 0:
        last_data_speed = data_processing(err_data_speed)
        last_data_cur = data_processing(err_data_cur)
        last_change_mode = change_mode


# 驱动电机以正弦轨迹运动(测试模式需要连接USB以正常读取测试数据)
def run_sin(mid, angle, speed):
    global start_flag
    dr_can.set_angle(mid, 0, 5, 5, 1)
    time.sleep(0.5)
    while True:
        if not start_flag and not test_flag:
            start_flag = False
            dr_can.set_mode(mid, 1)
            return
        a = dr_can.read_property(mid, 'axis0.controller.trajectory_done')
        if a == 1:
            break
    points = speed / (1 / frequency_HZ)
    for i in range(int(points)):
        if not start_flag and not test_flag:
            start_flag = False
            dr_can.set_mode(mid, 1)
            return
        n_angle = angle * 0.5 * (sin((i + 1) * 2 * 3.1415 / points - 1.57075) + 1)
        dr_can.set_angle(id_num=mid, angle=n_angle, speed=30, param=frequency_HZ / 2, mode=0)
        time.sleep(1 / frequency_HZ)
    if start_flag:
        time.sleep(0.5)
        dr_can.set_mode(mid, 1)
        button_setZero.configure(text="设置零点")
        start_flag = False


def Automated_test():
    global aut_flag
    aut_flag = True
    button_startTest_command(1)

# 带载测试(位置模式)
def test_OnLoad(mid, test, speed, pos):
    global test_flag, err_data_pos, err_data_cur, err_data_speed, save_flag
    global last_data_speed, last_data_cur, last_change_mode,aut_flag
    a = 0
    b = 0
    if test != 0:
        err_data_cur.clear()
        err_data_speed.clear()
        err_data_pos.clear()
        NNmsg_entry.delete(0, END)
        if combobox_traj.current() == 0:  # 梯形轨迹模式
            speed_a = speed ** 2 / (60 * pos / 360)
            if speed_a > 327.67:
                test_flag = False
                button_setZero.configure(text='设置零点')
                MSG.showerror('错误', f"当前速度过大，当前{pos}角度最大速度为{sqrt(327.67 * (60 * pos / 360))}")
                return
            # a最大为327.67，则speed最大为根号下327.67*（60*pos/360）
            dr_can.set_angle(mid, pos, speed, speed_a, 1)
            time.sleep(0.5)
            while True:
                if not test_flag:
                    test_flag = False
                    button_setZero.configure(text='设置零点')
                    MSG.showinfo('提示', '测试已退出')
                    return
                # 检查是否达到目标位置，切换状态
                if a == 0:
                    a = dr_can.read_property(mid, 'axis0.controller.trajectory_done')
                # print('a' + str(a))
                if a == 1:
                    # time.sleep(0.5)
                    dr_can.set_angle(mid, 0, speed, speed_a, 1)
                    a = 2
                if a == 2:
                    b = dr_can.read_property(mid, 'axis0.controller.trajectory_done')
                # print('b' + str(b))
                if b == 1:
                    break
                # 数据读取
                read_data()
            test_flag = False

        elif combobox_traj.current() == 1:  # 正弦轨迹模式
            if not usbCom_flag:
                MSG.showerror('错误', '正弦测试需开启USB')
                return
            temp = dr_can.read_property(mid, 'axis0.motor_thermistor.temperature')
            if temp >= temp_limit:
                MSG.showerror('错误', f"电机温度较高({'%.2f' % temp}℃)，请冷却后再试")
                test_flag = False
                NNmsg_entry.delete(0, END)
                button_setZero.configure(text='设置零点')
                return
            test_flag = True
            USBread_data()
            run_sin(mid, pos, speed)
            if test_flag:
                run_sin(mid, pos, speed)
            test_flag = False
            time.sleep(0.5)

        dr_can.set_mode(mid, 1)
        print(time.strftime('%Y-%m-%d %H:%M:%S'))
        print(
            f'''{data_processing(err_data_speed)} 
{data_processing(err_data_cur)} 
{data_processing(err_data_pos)} 
gain:{'%0.3f' % vel_gain} inte:{'%0.3f' % vel_inte}''')
        # MSG.showinfo('提示', '测试完成,等待电机降温')
        temp = dr_can.read_property(mid, 'axis0.motor_thermistor.temperature')
        while temp >= temp_limit:
            NNmsg_entry.delete(0, END)
            NNmsg_entry.insert(END, f"电机温度过高（{'%.02f' % temp}℃）请等待")
            time.sleep(1)
            temp = dr_can.read_property(mid, 'axis0.motor_thermistor.temperature')
        NNmsg_entry.delete(0, END)
        button_setZero.configure(text='设置零点')
    else:
        if combobox_traj.current() == 0:
            dr_can.set_angle(mid, pos, speed, speed, 1)
        if combobox_traj.current() == 1:
            run_sin(mid, pos, speed)
    if save_flag and excel_en.get() == 1:
        button_save_command()
        print("当前的参数类型："+str(change_mode))
        # save_flag = False
        if aut_flag:
            startMotor()

    elif test != 0:
        last_data_speed = data_processing(err_data_speed)
        last_data_cur = data_processing(err_data_cur)
        last_change_mode = change_mode
        if aut_flag:
            updata_vel_gain(1)


# 空载测试(速度模式)
def test_OutLoad(mid, test, speed):
    global test_flag, save_flag, last_data_speed, last_data_cur, last_change_mode

    dr_can.set_speed(id_num=mid, speed=speed, param=0, mode=1)
    time.sleep(1)
    if test != 0:
        err_data_cur.clear()
        err_data_speed.clear()
        err_data_pos.clear()
        for i in range(TEST_TIMES):
            if not test_flag:
                MSG.showinfo('提示', '测试已退出')
                return
            # 数据读取
            read_data()
            if i > 10 and (err_data_speed[-1] > (speed / 10)) and speed != 0:
                break

        dr_can.set_mode(mid, 1)
        test_flag = False
        button_setZero.configure(text='设置零点')
        print(time.strftime('%Y-%m-%d %H:%M:%S'))
        print(data_processing(err_data_speed))
        MSG.showinfo('提示', '测试完成')

    if save_flag and excel_en.get() == 1:
        button_save_command()
        save_flag = False
    elif test != 0:
        last_data_speed = data_processing(err_data_speed)
        last_data_cur = data_processing(err_data_cur)
        last_change_mode = change_mode


# 获取界面设置的最大扭矩
def get_cur():
    str_cur = cur_entry.get()
    # if not Com_flag:
    # 	MSG.showerror('错误', '串口未打开！')
    # 	return ''
    if str_cur != '' and str_cur.isdigit():
        return int(str_cur)
    elif str_cur.count(".") == 1 and not str_cur.startswith(".") and not str_cur.endswith("."):
        return float(str_cur)
    else:
        MSG.showerror('错误', '电流输入错误，请输入纯数字')
        return ''


# 获取界面设置的速度
def get_speed():
    str_speed = speed_entry.get()
    # if not Com_flag:
    # 	MSG.showerror('错误', '串口未打开！')
    # 	return ''
    if run_mode == 2:
        k = ratio
    else:
        k = 1
    if str_speed != '' and str_speed.isdigit():
        return float(str_speed) / k
    elif str_speed.count(".") == 1 and not str_speed.startswith(".") and not str_speed.endswith("."):
        return float(str_speed) / k
    else:
        MSG.showerror('错误', '速度输入错误，请输入纯数字')
        return ''


# 获取界面设置的角度
def get_pos():
    str_pos = pos_entry.get()
    # if not Com_flag:
    # 	MSG.showerror('错误', '串口未打开！')
    # 	return ''
    if str_pos != '' and str_pos.isdigit():
        return int(str_pos)
    elif str_pos.count(".") == 1 and not str_pos.startswith(".") and not str_pos.endswith("."):
        return float(str_pos)
    else:
        MSG.showerror('错误', '角度输入错误，请输入纯数字')
        return ''


# 获取界面设置要控制的电机ID
def get_id():
    str_id = id_entry.get()
    if str_id != '' and str_id.isdigit():
        m_id = int(str_id)
        if Com_flag:
            if m_id != 0:
                if not dr_can.get_id(m_id) == m_id:
                    MSG.showerror('错误', '无法连接指定ID电机，请检查ID号')
                    return ''
        return m_id
    else:
        MSG.showerror('错误', 'ID输入错误，请输入整数')
        return ''


# 开始一次测试
def button_startTest_command(test):
    global test_flag
    if (not test_flag) or start_flag:
        motor_id = get_id()
        if not Com_flag:
            MSG.showerror('错误', '串口未打开！')
            return
        if motor_id == '':
            return
        time.sleep(0.1)
        if run_mode == 1:
            cur = get_cur()
            if cur == '':
                return
            Thread(target=test_cur, args=(motor_id, test, cur,)).start()

        if run_mode == 2:
            speed = get_speed()
            if speed == '':
                return
            Thread(target=test_OutLoad, args=(motor_id, test, speed,)).start()

        if run_mode == 3:
            speed = get_speed()
            if speed == '':
                return
            pos = get_pos()
            if pos == '':
                return
            Thread(target=test_OnLoad, args=(motor_id, test, speed, pos)).start()

        if test != 0:
            test_flag = True
        button_setZero.configure(text='待机')
    else:
        MSG.showerror('错误', '请等待测试完成！')


# 保存数据到Excel
def button_save_command():
    global test_flag, err_data_speed, err_data_cur, err_data_pos, change_mode, save_flag
    global last_data_speed, last_data_cur, last_change_mode
    if not test_flag:
        if change_mode == 0:
            MSG.showerror('错误', '数据并无更改')
            return

        def m_abs(a):
            if a < 0:
                return -a
            return a

        data_speed = data_processing(err_data_speed)
        data_cur = data_processing(err_data_cur)
        print(last_data_speed)
        print(data_speed)
        print('*******')
        print(last_data_cur)
        print(data_cur)
        if last_change_mode != 0:
            err = 0
            for i in range(4):
                if m_abs(data_speed[i]) <= m_abs(last_data_speed[i]):
                    err = err + 1
            for i in range(4):
                if m_abs(data_cur[i]) <= m_abs(last_data_cur[i]):
                    err = err + 1
            if err <= 3:
                # mess = MSG.askyesno(message='此次修改并无改善，是否保存？', default=MSG.NO)
                # if not mess:
                NNmsg_entry.delete(0, END)
                NNmsg_entry.insert(END, '此次修改不写入！')
                last_data_speed = data_speed.copy()
                last_data_cur = data_cur.copy()
                last_change_mode = change_mode
                # change_mode = 0
                save_flag = False
                return False

            data = [change_mode - 1] + last_data_speed + last_data_cur + [
                vel_gain, vel_inte, cur_band, pos_gain] + [get_step()] + [time.strftime('%Y-%m-%d %H:%M')]

        else:
            data = [change_mode - 1] + data_speed + data_cur + [
                vel_gain, vel_inte, cur_band, pos_gain] + [get_step()] + [time.strftime('%Y-%m-%d %H:%M')]

        fileName = excelname_entry.get()
        if os.path.exists(fileName):
            book = openpyxl.load_workbook(fileName)
            sheet = book.active
        else:
            book = openpyxl.Workbook()
            sheet = book.active
            sheet.append(['change',
                          'V_max', 'V_min', 'V_mean', 'V_var',
                          'i_max', 'i_min', 'i_mean', 'i_var',
                          'vel_gain', 'vel_inte', 'cur_band', 'pos_gain',
                          'step', 'time'])

        sheet.append(data)
        print('当前行数:' + str(sheet.max_row))
        errorTimes = 0
        while True:
            try:
                if errorTimes == 3:
                    MSG.showerror(f'错误', f'多次写入失败！')
                    return False
                # if change_mode == 4:
                #     print("---------写入表格---------")
                book.save(fileName)
                break
            except Exception:
                errorTimes = errorTimes + 1
                MSG.showerror(f'错误', f'文件{fileName}写入失败！请关闭文件后再试')
        NNmsg_entry.delete(0, END)
        NNmsg_entry.insert(END, f'文件{fileName}已写入成功！')
        last_data_speed = data_speed.copy()
        last_data_cur = data_cur.copy()
        last_change_mode = change_mode
        # change_mode = 0
        save_flag = True
        return True

    else:
        MSG.showerror('错误', '请等待测试完成！')
        return False


# 保存运行模式
def button_savemode_command():
    speed = get_speed()
    if speed == '':
        MSG.showerror(message="速度/周期值错误！")
        return
    pos = get_pos()
    if pos == '':
        MSG.showerror(message="角度值错误！")
        return
    data_mode = [speed, pos, run_mode, combobox_traj.current()]
    numpy.savetxt(r'data_mode.txt', data_mode, fmt="%s")
    MSG.showinfo(message="已保存！")


# 从电机读取所有参数
def read_alldata():
    global text_pos_gain, text_vel_gain, text_vel_inte, text_cur_band
    if not Com_flag:
        MSG.showerror('错误', '串口未打开！')
        return
    motor_id = get_id()
    if motor_id == '':
        return
    text_vel_gain.delete(1.0, END)
    text_vel_inte.delete(1.0, END)
    text_pos_gain.delete(1.0, END)
    text_cur_band.delete(1.0, END)

    global vel_gain, vel_inte, pos_gain, cur_band
    vel_gain = dr_can.read_property(motor_id, 'axis0.controller.config.vel_gain')
    vel_inte = dr_can.read_property(motor_id, 'axis0.controller.config.vel_integrator_gain')
    pos_gain = dr_can.read_property(motor_id, 'axis0.controller.config.pos_gain')
    cur_band = dr_can.read_property(motor_id, 'axis0.motor.config.current_control_bandwidth')
    try:
        text_vel_gain.insert(END, '%0.3f' % vel_gain)
        text_vel_inte.insert(END, '%0.3f' % vel_inte)
        text_pos_gain.insert(END, '%0.3f' % pos_gain)
        text_cur_band.insert(END, '%0.3f' % cur_band)
        return True
    except Exception:
        return False


ratio = 0  # 电机减速比


# CAN连接
def button_ComOpen_command():
    global Com_flag, ratio, vel_gain
    if not Com_flag:
        com = plist[combobox_coms.current()][0]
        if com == '':
            MSG.showerror('错误', '请选择正确的串口号！')
            return

        str_banudrate = banudrate_entry.get()
        if str_banudrate != '' and str_banudrate.isdigit():
            banudrate = int(str_banudrate)
        else:
            MSG.showerror('错误', '波特率输入错误，请输入纯数字！')
            return
        try:
            dr_can.COM_init(com, banudrate)
        except Exception:
            MSG.showerror('错误', '串口打开失败！')
            return

        if dr_can.uart == 0:
            MSG.showerror('错误', '串口打开失败！')
            return
        motor_id = get_id()
        if motor_id == '':
            return
        Com_flag = True
        if not read_alldata():
            MSG.showerror('错误', 'CAN通信失败,请检查波特率和COM是否选择正确！')
            dr_can.uart.close()
            Com_flag = False
            return
        button_ComOpen.configure(text='断开CAN')
        # 电机待机
        dr_can.set_mode(motor_id, 1)
        dr_can.set_zero_position(motor_id)
        # 关闭电机软件限位

        dr_can.write_property(id_num=motor_id, property='axis0.config.extra_setting.enable_circular_setpoint_limit',
                              value=1)
        dr_can.write_property(id_num=motor_id, property='axis0.output_shaft.circular_setpoint_min',
                              value=-180)
        dr_can.write_property(id_num=motor_id, property='axis0.output_shaft.circular_setpoint_max',
                              value=180)
        # 关闭电机多圈计数
        dr_can.write_property(id_num=motor_id, property='axis0.config.extra_setting.enable_multi_circle', value=1)
        dr_can.save_config(id_num=motor_id)
        print("设置软件限位")
        print(change_mode)
        # 清除错误
        dr_can.clear_error(id_num=motor_id)
        max_speed = dr_can.read_property(motor_id, 'axis0.output_shaft.vel_limit')
        ratio = dr_can.read_property(motor_id, 'axis0.config.extra_setting.gear_ratio')
        try:
            float(speed_entry.get())
        except Exception:
            default_speed = 1000 / ratio
            if run_mode == 2:
                if max_speed < default_speed:
                    speed_entry.delete(0, END)
                    speed_entry.insert(END, '%.0f' % float(max_speed * ratio))
                else:
                    speed_entry.delete(0, END)
                    speed_entry.insert(END, '1000')
            if run_mode == 3 and combobox_traj.current() == 1:
                speed_entry.delete(0, END)
                speed_entry.insert(END, '1')
            else:
                if max_speed < default_speed:
                    speed_entry.delete(0, END)
                    speed_entry.insert(END, '%.0f' % float(max_speed))
                else:
                    speed_entry.delete(0, END)
                    speed_entry.insert(END, '%.0f' % float(default_speed))
    else:
        dr_can.uart.close()
        button_ComOpen.configure(text='连接CAN')
        Com_flag = False


usbCom_flag = False  # USB连接状态


# USB连接
def button_usbOpen_command():
    global usbCom_flag, ratio, Type_LL, frequency_HZ
    if not usbCom_flag:
        com = plist[combobox_coms.current()][0]
        if com == '':
            MSG.showerror('错误', '请选择正确的串口号！')
            return
        motor_id = get_id()
        if motor_id == '':
            MSG.showerror('错误', '请正确输入ID！')
            return
        str_banudrate = banudrate_entry.get()
        if str_banudrate != '' and str_banudrate.isdigit():
            banudrate = int(str_banudrate)
        else:
            MSG.showerror('错误', '波特率输入错误，请输入纯数字！')
            return

        try:
            dr_asc.COM_init(com, banudrate)
        except Exception:
            MSG.showerror('错误', '串口打开失败，请重新接上数据线后再尝试')
            return
        if dr_asc.uart == 0:
            MSG.showerror('错误', '串口打开失败，请重新接上数据线后再尝试')
            return
        try:
            int(dr_asc.get_id(motor_id))
            frequency_HZ = frequency_HZ_ST
            Type_LL = False
        except Exception:
            dr_asc.uart.close()
            dr_asc.uart = 0

            # 尝试连接uart模块（低成本方案）
            try:
                dr_uart.COM_init(com, banudrate)
            except Exception:
                MSG.showerror('错误', '串口打开失败，请重新接上数据线后再尝试')
                return
            if dr_uart.uart == 0:
                MSG.showerror('错误', '串口打开失败，请重新接上数据线后再尝试')
                return
            try:
                int(dr_uart.get_id(motor_id))
                frequency_HZ = frequency_HZ_LL
                Type_LL = True
            except Exception:
                dr_uart.uart.close()
                dr_uart.uart = 0
                MSG.showerror('错误', 'USB通信失败，请确认COM选择是否正确')
                return

        usbCom_flag = True
        button_usbOpen.configure(text='断开USB')

    else:
        if Type_LL:
            dr_uart.uart.close()
            dr_uart.uart = 0
        else:
            dr_asc.uart.close()
            dr_asc.uart = 0
        button_usbOpen.configure(text='连接USB')
        usbCom_flag = False


# 获取文本框内容
def get_text_data(text):
    str_value = text.get(1.0, END).rstrip()
    str_value.lstrip()
    if str_value != '' and str_value.isdigit():
        return int(str_value)
    elif str_value.count(".") == 1 and not str_value.startswith(".") and not str_value.endswith("."):
        return float(str_value)
    else:
        MSG.showerror('错误', '数据输入错误，请输入数字')
        return 0


# 读取界面显示的所有闭环参数
def updata_all():
    global vel_gain, vel_inte, pos_gain, cur_band
    vel_gain = get_text_data(text_vel_gain)
    vel_inte = get_text_data(text_vel_inte)
    pos_gain = get_text_data(text_pos_gain)
    cur_band = get_text_data(text_cur_band)


# 更新所有闭环参数到电机
def button_updata_command():
    global vel_gain, vel_inte, pos_gain, cur_band
    if not Com_flag:
        MSG.showerror('错误', '串口未打开！')
        return
    motor_id = get_id()
    if motor_id == '':
        return
    updata_all()
    dr_can.write_property(motor_id, 'axis0.controller.config.vel_gain', vel_gain)
    dr_can.write_property(motor_id, 'axis0.controller.config.vel_integrator_gain', vel_inte)
    dr_can.write_property(motor_id, 'axis0.controller.config.pos_gain', pos_gain)
    dr_can.write_property(motor_id, 'axis0.motor.config.current_control_bandwidth', cur_band)
    dr_can.save_config(motor_id)
    # MSG.showinfo('提示', '数据写入完成')
    NNmsg_entry.delete(0,END)
    NNmsg_entry.insert(END,"========数据写入完成========")


# 获取步长值
def get_step():
    global step_entry
    str_value = step_entry.get()
    if str_value.isdigit():
        return int(str_value)
    else:
        return 0


save_flag = False


# 更新速度增益到电机
def updata_vel_gain(a):
    if test_flag:
        MSG.showerror('错误', '请等待测试完成！')
        return
    step = get_step()
    if step == 0:
        MSG.showerror('错误', '请正确输入步长！（整数）')
        return
    global vel_gain, text_vel_gain, change_mode, save_flag
    mid = get_id()
    time.sleep(0.1)
    temp = dr_can.read_property(mid, 'axis0.motor_thermistor.temperature')
    if temp >= temp_limit:
        MSG.showerror('错误', f"电机温度较高({'%.2f' % temp}℃)，请冷却后再试")
        return
    if a == 0:
        change_mode = 2
        vel_gain = vel_gain - vel_gain * step / 100
        if vel_gain < 0:
            vel_gain = 0

    else:
        change_mode = 1
        if vel_gain == 0:
            vel_gain = 0.01
        else:
            vel_gain = vel_gain + vel_gain * step / 100

    text_vel_gain.delete(1.0, END)
    text_vel_gain.insert(END, '%0.3f' % vel_gain)
    dr_can.write_property(mid, 'axis0.controller.config.vel_gain', vel_gain)
    dr_can.save_config(mid)
    save_flag = True
    button_startTest_command(1)


# 更新速度积分到电机
def updata_vel_inte(a):
    if test_flag:
        MSG.showerror('错误', '请等待测试完成！')
        return
    step = get_step()
    if step == 0:
        MSG.showerror('错误', '请正确输入步长！（整数）')
        return
    global vel_inte, text_vel_inte, change_mode, save_flag
    mid = get_id()
    time.sleep(0.1)
    temp = dr_can.read_property(mid, 'axis0.motor_thermistor.temperature')
    if temp >= temp_limit:
        MSG.showerror('错误', f"电机温度较高({'%.2f' % temp}℃)，请冷却后再试")
        return
    if a == 0:
        change_mode = 4
        vel_inte = vel_inte - vel_inte * step / 100
        if vel_inte < 0:
            vel_inte = 0
    else:
        change_mode = 3
        if vel_inte == 0:
            vel_inte = 0.01
        else:
            vel_inte = vel_inte + vel_inte * step / 100
    text_vel_inte.delete(1.0, END)
    text_vel_inte.insert(END, '%0.3f' % vel_inte)
    dr_can.write_property(mid, 'axis0.controller.config.vel_integrator_gain', vel_inte)
    dr_can.save_config(mid)
    save_flag = True
    button_startTest_command(1)


# 更新位置增益到电机
def updata_pos_gain(a):
    if test_flag:
        MSG.showerror('错误', '请等待测试完成！')
        return
    step = get_step()
    if step == 0:
        MSG.showerror('错误', '请正确输入步长！（整数）')
        return
    global pos_gain, text_pos_gain, change_mode, save_flag
    mid = get_id()
    time.sleep(0.1)
    temp = dr_can.read_property(mid, 'axis0.motor_thermistor.temperature')
    if temp >= temp_limit:
        MSG.showerror('错误', f"电机温度较高({'%.2f' % temp}℃)，请冷却后再试")
        return
    if a == 0:
        change_mode = 6
        pos_gain = pos_gain - pos_gain * step / 100
        if pos_gain < 0:
            pos_gain = 0
    else:
        change_mode = 5
        if pos_gain == 0:
            pos_gain = 0.01
        else:
            pos_gain = pos_gain + pos_gain * step / 100
    text_pos_gain.delete(1.0, END)
    text_pos_gain.insert(END, '%0.3f' % pos_gain)
    dr_can.write_property(mid, 'axis0.controller.config.pos_gain', pos_gain)
    dr_can.save_config(mid)
    save_flag = True
    button_startTest_command(1)


# 更新电流带宽到电机
def updata_cur_band(a):
    step = get_step()
    if step == 0:
        MSG.showerror('错误', '请正确输入步长！（整数）')
        return
    global cur_band, text_cur_band, change_mode, save_flag
    mid = get_id()
    time.sleep(0.1)
    temp = dr_can.read_property(mid, 'axis0.motor_thermistor.temperature')
    if temp >= temp_limit:
        MSG.showerror('错误', f"电机温度较高({'%.2f' % temp}℃)，请冷却后再试")
        return
    if a == 0:
        change_mode = 8
        cur_band = cur_band - cur_band * step / 100
        if cur_band < 0:
            cur_band = 0
    else:
        change_mode = 7
        if cur_band == 0:
            cur_band = 10
        else:
            cur_band = cur_band + cur_band * step / 100
    text_cur_band.delete(1.0, END)
    text_cur_band.insert(END, '%0.3f' % cur_band)
    dr_can.write_property(mid, 'axis0.motor.config.current_control_bandwidth', cur_band)
    dr_can.save_config(mid)
    save_flag = True
    button_startTest_command(1)


# 电机运行模式选择
def mode_slect():
    global var, run_mode
    if run_mode == 2:
        run_mode = var.get()
        speed = get_speed()
        speed_entry.delete(0, END)
        speed_entry.insert(END, '%.1f' % (speed / ratio))
    else:
        speed = get_speed()
        run_mode = var.get()
        if run_mode == 2:
            speed_entry.delete(0, END)
            speed_entry.insert(END, '%.1f' % (speed * ratio))
    if run_mode == 3:
        sel = combobox_traj.current()
        if sel == 1:
            label_parm1.configure(text='周期:')
        if sel == 0:
            label_parm1.configure(text='速度:')
    else:
        label_parm1.configure(text='速度:')


start_flag = False

def startMotor(motor_id=0):
    global start_flag, number, last_change_mode, change_mode,aut_flag
    if not Com_flag:
        MSG.showerror('错误', '串口未打开！')
        return
    if test_flag:
        MSG.showerror('错误', '请等待测试完成！')
        return
    try:
        if save_flag and change_mode == 1:
            updata_vel_gain(1)

        elif save_flag and change_mode == 4:
            updata_vel_inte(0)

        elif not save_flag and change_mode == 4:
            updata(2)
            button_save_command()
            last_change_mode = change_mode
            change_mode = 1
            # MSG.showinfo('提示', '测试结束')
            a = [0, 0.5, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
            text_vel_gain.delete(1.0, END)
            text_vel_inte.delete(1.0, END)
            vel_gain = vel_gain_initial
            vel_inte = a[number]

            text_vel_gain.insert(END, '%0.3f' % vel_gain)
            text_vel_inte.insert(END, '%0.3f' % vel_inte)
            dr_can.write_property(motor_id, 'axis0.controller.config.vel_gain', vel_gain)
            dr_can.write_property(motor_id, 'axis0.controller.config.vel_integrator_gain', vel_inte)
            number = number - 1
            if number == -1:
                text_vel_gain.delete(1.0, END)
                text_vel_inte.delete(1.0, END)
                MSG.showinfo('提示', '调试完成！')
                number = 10
                aut_flag  = False
            else:
                button_updata_command()
                button_startTest_command(1)
        elif not save_flag and change_mode == 1:
            updata(1)
            button_save_command()
            updata_vel_inte(0)
    except Exception as e:
        print("error: ", e)
        error = "error: {}".format(e)
        MSG.showerror('error', error)

# 启动GUI（非测试模式）
def button_startMotor_command(motor_id=0):
    global start_flag, vel_inte
    if not Com_flag:
        MSG.showerror('错误', '串口未打开！')
        return
    if test_flag:
        MSG.showerror('错误', '请等待测试完成！')
        return
    speed_curve = []
    speed_target = []
    position1 = []
    position_current1 = []
    max_speed = dr_can.read_property(motor_id, 'axis0.output_shaft.vel_limit')
    print(max_speed)
    dr_can.set_angle(id_num=motor_id, angle=150, speed=max_speed *2/3, param=10, mode=1)
    while dr_can.read_property(id_num=motor_id, property="axis0.controller.trajectory_done") != 1:
        speed_curve.append(abs(dr_can.read_property(id_num=motor_id, property="axis0.controller.vel_setpoint")))
        speed_target.append(abs(dr_can.read_property(id_num=motor_id, property="axis0.encoder.vel_estimate")))
        position1.append(dr_can.read_property(id_num=motor_id, property="axis0.controller.input_pos"))
        position_current1.append(dr_can.read_property(id_num=motor_id, property="axis0.encoder.pos_estimate"))
    time.sleep(1)
    dr_can.set_angle(id_num=motor_id, angle=0, speed=10, param=10, mode=1)
    # print(position_current1)
    # position = ascii.read_property(id_num=0, property="axis0.controller.input_pos")
    # position_current = ascii.read_property(id_num=0, property="axis0.encoder.pos_estimate")
    plt.subplot(2, 1, 1)
    x1 = np.arange(0, len(position1), 1)
    y3 = position1
    y4 = position_current1
    plt.plot(x1, y3, label="position1")
    plt.plot(x1, y4, label="position_current1")
    plt.legend()
    plt.grid(1)
    # plt.savefig('位置曲线.png')
    # plt.show()
    plt.subplot(2, 1, 2)
    x = np.arange(0, len(speed_curve), 1)
    y1 = speed_curve
    y2 = speed_target
    plt.plot(x, y1, label="speed_curve")
    plt.plot(x, y2, label="speed_target")
    plt.savefig('./doc/位置速度曲线.png')
    plt.legend()
    plt.grid(1)
    plt.show()
    # print("目标位置" + str(position*10))
    # print("当前位置" + str(position_current*10))
    # print(position1)
    # print(position_current1)
    # print(speed_curve)
    # print(speed_target)
    # # start_flag = True
    # # button_startTest_command(1)


# button_setZero.configure(text="待机")


# 重置电机零点/待机
def button_setZero_command():
    global start_flag, test_flag
    if not Com_flag:
        MSG.showerror('错误', '串口未打开！')
        return
    motor_id = get_id()
    if motor_id == '':
        return
    if test_flag or start_flag:
        dr_can.set_mode(motor_id, 1)
        button_setZero.configure(text="设置零点")
        test_flag = False
        start_flag = False
    else:
        dr_can.set_zero_position(motor_id)
        MSG.showinfo('提示', '零点设置完成')


# 随机参数测试
def buttono_randTset_command(test):
    global vel_gain, vel_inte, text_pos_gain, text_vel_gain, last_change_mode
    if not Com_flag:
        MSG.showerror('错误', '串口未打开！')
        return
    motor_id = get_id()
    if motor_id == '':
        return

    text_vel_gain.delete(1.0, END)
    text_vel_inte.delete(1.0, END)
    vel_gain = random.uniform(0, rand_gain)

    vel_inte = random.uniform(0, rand_inte)
    dr_can.write_property(motor_id, 'axis0.controller.config.vel_gain', vel_gain)
    dr_can.write_property(motor_id, 'axis0.controller.config.vel_integrator_gain', vel_inte)
    text_vel_gain.insert(END, '%0.3f' % vel_gain)
    text_vel_inte.insert(END, '%0.3f' % vel_inte)
    last_change_mode = 0
    if test != 0:
        button_startTest_command(1)


# 测试最大扭矩
def tor_test_max():
    global test_flag

    def test():
        global test_flag
        if not Com_flag:
            MSG.showerror('错误', '串口未打开')
            return
        max_tor = 0
        motor_id = get_id()
        if motor_id == '':
            return
        while True:
            if not test_flag:
                dr_can.set_mode(motor_id, 1)
                return
            dr_can.set_torque(id_num=motor_id, torque=max_tor)
            time.sleep(0.05)
            angle = 0
            for i in range(20):
                angle = float(dr_can.read_property(motor_id, 'axis0.output_shaft.pos_estimate'))
                if angle >= 90:
                    break
            if angle >= 90:
                if max_tor > 50:
                    max_tor = int(max_tor * 0.8)
                else:
                    max_tor = int(max_tor * 0.9)
                print(f'最大扭矩值:{max_tor}')
                dr_can.set_torque(id_num=motor_id, torque=max_tor / 2)
                time.sleep(1)
                dr_can.set_torque(id_num=motor_id, torque=max_tor / 5)
                time.sleep(1)
                dr_can.set_torque(id_num=motor_id, torque=max_tor / 10)
                time.sleep(1)
                dr_can.set_mode(motor_id, 1)
                break
            max_tor = max_tor + 0.5
            cur_entry.delete(0, END)
            cur_entry.insert(END, '%.1f' % max_tor)

        cur_entry.delete(0, END)
        cur_entry.insert(END, '%.1f' % max_tor)
        test_flag = False
        button_setZero.configure(text="设置零点")

    def test_new():
        global test_flag
        if not Com_flag:
            MSG.showerror('错误', '串口未打开')
            return
        max_tor = 0
        motor_id = get_id()
        if motor_id == '':
            return

        if not test_flag:
            dr_can.set_mode(motor_id, 1)
            return
        torque_list = []
        dr_can.set_angle(id_num=motor_id, angle=150, speed=10, param=10, mode=1)
        time.sleep(5)
        dr_can.set_angle(id_num=motor_id, angle=90, speed=10, param=10, mode=1)
        time.sleep(30)
        for i in range(10):
            value = dr_can.read_property(id_num=motor_id, property='axis0.output_shaft.torque_estimate')
            torque_list.append(value)
            time.sleep(0.01)
        print(torque_list)
        max_tor = np.mean(torque_list)
        cur_entry.delete(0, END)
        cur_entry.insert(END, '%.1f' % max_tor)
        dr_can.set_angle(id_num=motor_id, angle=0, speed=10, param=10, mode=1)

    button_setZero.configure(text='待机')
    test_flag = True
    Thread(target=test).start()


# 获取输入框内容
def get_entryData(entry):
    str_data = entry.get()
    if run_mode == 2:
        k = ratio
    else:
        k = 1
    if str_data != '' and str_data.isdigit():
        return float(str_data) / k
    elif str_data.count(".") == 1 and not str_data.startswith(".") and not str_data.endswith("."):
        return float(str_data) / k
    else:
        return ''


# 带宽测试
def tor_test():
    global test_flag

    def testThread():
        global test_flag
        max_tor = get_entryData(cur_entry)
        mbandwidth = get_entryData(curbandL_entry)
        max_band = get_entryData(curbandH_entry)
        stp_band = get_entryData(curbandS_entry)
        mfileName = xlsx_entry.get()

        if max_tor > 20:
            stp_tor = 0.5
        else:
            stp_tor = 0.25
        times = int(max_tor / stp_tor)
        tor_err = []

        motor_id = get_id()
        if motor_id == '':
            return

        # 进行一次测试
        def test_once():
            tors = []
            for i in range(times):
                if not test_flag:
                    MSG.showinfo('提示', '测试已退出')
                    return

                dr_can.set_torque(id_num=motor_id, torque=(i + 1) * stp_tor)
                tors.append(-dr_can.read_property(motor_id, 'axis0.output_shaft.torque_estimate'))
                time.sleep(0.04)
            for i in range(times):
                if not test_flag:
                    MSG.showinfo('提示', '测试已退出')
                    return
                dr_can.set_torque(id_num=motor_id, torque=(times - i - 1) * stp_tor)
                tors.append(-dr_can.read_property(motor_id, 'axis0.output_shaft.torque_estimate'))
                time.sleep(0.05)
            dr_can.set_mode(motor_id, 1)

            for i in range(times):
                tor_err.append(tors[i] - (i + 1) * stp_tor)
            for i in range(times):
                tor_err.append(tors[times + i] - (times - i - 1) * stp_tor)

        while True:
            if not test_flag:
                dr_can.set_mode(motor_id, 1)
                break
            dr_can.write_property(motor_id, 'axis0.motor.config.current_control_bandwidth', mbandwidth)
            tor_err.clear()
            for i in range(5):
                if not test_flag:
                    MSG.showinfo('提示', '测试已退出')
                    return
                print(f'带宽{mbandwidth}:第{i + 1}次测试')
                test_once()
                time.sleep(0.5)
            data_mean = np.mean(tor_err)
            data_var = np.var(tor_err)
            print(f'mean:{"%.6f" % data_mean} var:{"%.6f" % data_var}')

            if os.path.exists(mfileName):
                book = openpyxl.load_workbook(mfileName)
                sheet = book.active
            else:
                book = openpyxl.Workbook()
                sheet = book.active
                sheet.append(['电流带宽', '误差均值', '误差方差'])
            data = [mbandwidth, data_mean, data_var]
            sheet.append(data)
            book.save(mfileName)

            mbandwidth = mbandwidth + stp_band
            if mbandwidth > max_band:
                break
        button_setZero.configure(text='待机')
        test_flag = False
        button_setZero.configure(text="设置零点")
        MSG.showinfo('提示', '测试结束')

    test_flag = True
    button_setZero.configure(text='待机')
    Thread(target=testThread).start()


nn = NN_BP.BPNeuralNetwork()


# 获取参数路径
def get_path():
    global path_flag
    path = filedialog.askdirectory(title='请选择参数集路径')
    NN_BP.dataPath = path
    if nn.readdata():
        numin = nn.input_weights.shape[0]
        NNin_entry.delete(0, END)
        NNin_entry.insert(END, numin)

        numhid = nn.input_weights.shape[1]
        NNhid_entry.delete(0, END)
        NNhid_entry.insert(END, numhid)

        speed_entry.delete(0, END)
        speed_entry.insert(END, int(nn.data_mode[0]))

        pos_entry.delete(0, END)
        pos_entry.insert(END, int(nn.data_mode[1]))

        var.set(int(nn.data_mode[2]))
        combobox_traj.current(int(nn.data_mode[3]))

        nn.setup(numin, numhid, 4)
        path_flag = True
    else:
        path_flag = False
        MSG.showerror('错误', '未找到数据文件，请重新选择目录')

    NNpath_entry.delete(0, END)
    NNpath_entry.insert(END, path)


path_flag = False
change = ''


# 根据预测结果更新闭环参数
def updata(m):
    global vel_gain, vel_inte,save_flag
    mchange = ''
    step = get_step()
    motor_id = get_id()
    if motor_id == '':
        return
    if m == 3:
        mchange = '降低速度积分'
        print('降低速度积分')
        vel_inte = vel_inte - vel_inte * step / 100
        if vel_inte < 0:
            vel_inte = 0
        dr_can.write_property(motor_id, 'axis0.controller.config.vel_integrator_gain', vel_inte)
        text_vel_inte.delete(1.0, END)
        text_vel_inte.insert(END, '%0.3f' % vel_inte)
    if m == 2:
        mchange = '升高速度积分'
        print('升高速度积分')
        if vel_inte == 0:
            vel_inte = 0.01
        else:
            vel_inte = vel_inte + vel_inte * step / 100
        dr_can.write_property(motor_id, 'axis0.controller.config.vel_integrator_gain', vel_inte)
        text_vel_inte.delete(1.0, END)
        text_vel_inte.insert(END, '%0.3f' % vel_inte)

    if m == 1:
        mchange = '降低速度增益'
        print('降低速度增益')
        vel_gain = vel_gain - vel_gain * step / 100
        if vel_gain < 0:
            vel_gain = 0
        dr_can.write_property(motor_id, 'axis0.controller.config.vel_gain', vel_gain)
        text_vel_gain.delete(1.0, END)
        text_vel_gain.insert(END, '%0.3f' % vel_gain)

    if m == 0:
        mchange = '增加速度增益'
        print('增加速度增益')
        if vel_gain == 0:
            vel_gain = 0.01
        else:
            vel_gain = vel_gain + vel_gain * step / 100
        dr_can.write_property(motor_id, 'axis0.controller.config.vel_gain', vel_gain)
        text_vel_gain.delete(1.0, END)
        text_vel_gain.insert(END, '%0.3f' % vel_gain)
    print(f"vel_gain:{'%.2f' % vel_gain} vel_inte:{'%.2f' % vel_inte}")
    return mchange


# 启动神经网络调参
def start_NNtest():
    global test_flag, start_flag
    motor_id = get_id()
    if motor_id == '':
        return

    def test_once(speed, pos, mode):  # mode1:正弦  0:梯形
        global test_flag, err_data_cur, err_data_speed, start_flag
        test_flag = True
        err_data_cur.clear()
        err_data_speed.clear()
        if mode == 1:
            USBread_data()

            dr_can.set_angle(motor_id, 0, 5, 5, 1)
            time.sleep(0.5)
            while True:
                if not start_flag and not test_flag:
                    dr_can.set_mode(motor_id, 1)
                    return
                a = dr_can.read_property(motor_id, 'axis0.controller.trajectory_done')
                if a == 1:
                    break
            points = speed / (1 / frequency_HZ)
            for i in range(int(points)):
                if not start_flag and not test_flag:
                    dr_can.set_mode(motor_id, 1)
                    return
                n_angle = pos * 0.5 * (sin((i + 1) * 2 * 3.1415 / points - 1.57075) + 1)
                dr_can.set_angle(id_num=motor_id, angle=n_angle, speed=30, param=frequency_HZ / 2, mode=0)
                time.sleep(1 / frequency_HZ)
            # run_sin(motor_id, pos, speed)
            if test_flag:
                dr_can.set_angle(motor_id, 0, 5, 5, 1)
                time.sleep(0.5)
                while True:
                    if not start_flag and not test_flag:
                        dr_can.set_mode(motor_id, 1)
                        return
                    a = dr_can.read_property(motor_id, 'axis0.controller.trajectory_done')
                    if a == 1:
                        break
                points = speed / (1 / frequency_HZ)
                for i in range(int(points)):
                    if not start_flag and not test_flag:
                        dr_can.set_mode(motor_id, 1)
                        return
                    n_angle = pos * 0.5 * (sin((i + 1) * 2 * 3.1415 / points - 1.57075) + 1)
                    dr_can.set_angle(id_num=motor_id, angle=n_angle, speed=30, param=frequency_HZ / 2, mode=0)
                    time.sleep(1 / frequency_HZ)
            # run_sin(motor_id, pos, speed)
            test_flag = False
        elif mode == 0:
            speed_a = speed ** 2 / (60 * pos / 360)
            if speed_a > 327.67:
                test_flag = False
                button_setZero.configure(text='设置零点')
                MSG.showerror('错误', f"当前速度过大，当前{pos}角度最大速度为{sqrt(327.67 * (60 * pos / 360))}")
                return
            # a最大为327.67，则speed最大为根号下327.67*（60*pos/360）
            dr_can.set_angle(motor_id, pos, speed, speed_a, 1)
            time.sleep(0.5)
            a = 0
            b = 0
            while True:
                if not test_flag:
                    test_flag = False
                    button_setZero.configure(text='设置零点')
                    MSG.showinfo('提示', '测试已退出')
                    return
                # 检查是否达到目标位置，切换状态
                if a == 0:
                    a = dr_can.read_property(motor_id, 'axis0.controller.trajectory_done')
                # print('a' + str(a))
                if a == 1:
                    # time.sleep(0.5)
                    dr_can.set_angle(motor_id, 0, speed, speed_a, 1)
                    a = 2
                if a == 2:
                    b = dr_can.read_property(motor_id, 'axis0.controller.trajectory_done')
                # print('b' + str(b))
                if b == 1:
                    break
                # 数据读取
                read_data()

        dr_can.set_mode(motor_id, 1)
        print(time.strftime('%Y-%m-%d %H:%M:%S'))
        print(f"{data_processing(err_data_speed)} {data_processing(err_data_cur)} "
              f"gain:{'%0.3f' % vel_gain} inte:{'%0.3f' % vel_inte}")
        return [np.max(err_data_speed), np.min(err_data_speed), np.mean(err_data_speed), np.var(err_data_speed),
                np.max(err_data_cur), np.min(err_data_cur), np.mean(err_data_cur), np.var(err_data_cur)]

    def NNtest():
        global change, test_flag, start_flag, Zero_error
        if not Com_flag:
            MSG.showerror('错误', 'CAN未连接！')
            return
        if not usbCom_flag:
            MSG.showerror('错误', '正弦测试需连接USB！')
            return
        if not path_flag:
            MSG.showerror('错误', '未加载参数集！')
            return
        times = get_entryData(NNtimes_entry)
        if times == '':
            MSG.showerror('错误', '请正确输入测试次数！')
            return
        speed = get_speed()
        if speed == '':
            MSG.showerror(message="速度/周期值错误！")
            return
        pos = get_pos()
        if pos == '':
            MSG.showerror('错误', "角度值错误！")
            return
        NNmsg_entry.delete(0, END)
        for i in range(int(times)):
            if not start_flag:
                dr_can.set_mode(motor_id, 1)
                button_setZero.configure(text='设置零点')
                return
            temp = dr_can.read_property(motor_id, 'axis0.motor_thermistor.temperature')
            msg = NNmsg_entry.get()
            while temp >= temp_limit:
                NNmsg_entry.delete(0, END)
                NNmsg_entry.insert(END, f"电机温度过高（{'%.02f' % temp}℃）请等待")
                time.sleep(1)
                temp = dr_can.read_property(motor_id, 'axis0.motor_thermistor.temperature')
            NNmsg_entry.delete(0, END)
            NNmsg_entry.insert(END, msg)
            if Zero_error:
                NNmsg_entry.delete(0, END)
                NNmsg_entry.insert(END, f"电机返回零点 请等待")
                dr_can.set_angle(motor_id, 0, 5, 5, 1)
                time.sleep(0.5)
                while True:
                    if not start_flag and not test_flag:
                        dr_can.set_mode(motor_id, 1)
                        return
                    a = dr_can.read_property(motor_id, 'axis0.controller.trajectory_done')
                    if a == 1:
                        break
                NNmsg_entry.delete(0, END)
                NNmsg_entry.insert(END, msg)
                Zero_error = False

            test_d = test_once(speed, pos, combobox_traj.current())
            data = np.row_stack((nn.test_data, [test_d]))
            data = NN_BP.min_max_normalization(data)
            in_d = data[-1]
            print(in_d)
            change = NN_BP.value_to_label(nn.predict(in_d))
            change = updata(change)
            if i == times - 1:
                NNmsg_entry.delete(0, END)
                NNmsg_entry.insert(END, "测试已结束" + change)
            else:
                NNmsg_entry.delete(0, END)
                NNmsg_entry.insert(END, f"已测试{i + 1}次:" + change)

            # 保存运行数据
            if True:
                excelname = NNxlsx_entry.get()
                data = data_processing(err_data_cur) + data_processing(err_data_speed) + [
                    vel_gain, vel_inte, change, time.strftime('%Y-%m-%d %H:%M')]
                if os.path.exists(excelname):
                    book = openpyxl.load_workbook(excelname)
                    sheet = book['原始数据']
                else:
                    book = openpyxl.Workbook()
                    sheet = book.active
                    sheet.title = '原始数据'
                    sheet.append(['i_max', 'i_min', 'i_mean', 'i_var',
                                  'V_max', 'V_min', 'V_mean', 'V_var',
                                  'vel_gain', 'vel_inte', 'change', 'time'])
                sheet.append(data)
                errorTimes = 0
                while True:
                    try:
                        if errorTimes == 3:
                            MSG.showerror(f'错误', f'多次写入失败！')
                            return False
                        book.save(excelname)
                        break
                    except Exception:
                        errorTimes = errorTimes + 1
                        MSG.showerror(f'错误', f'文件{excelname}写入失败！请关闭后再试')
        test_flag = False
        start_flag = False
        button_setZero.configure(text='设置零点')

    start_flag = True
    test_flag = True
    button_setZero.configure(text='待机')
    Thread(target=NNtest).start()


# 获取串口列表
plist = list(serial.tools.list_ports.comports())
COMS = []
for COM in plist:
    COMS.append(COM[1])
COMS.append('搜索串口')


# 选择串口时的回调事件（主要为了刷新串口列表）
def com_select(*args):
    global COMS, plist
    sel = combobox_coms.current()
    if sel == len(COMS) - 1:
        plist = list(serial.tools.list_ports.comports())
        COMS = []
        for com in plist:
            COMS.append(com[1])
        COMS.append('搜索串口')
        combobox_coms['values'] = COMS
        combobox_coms.current(0)


# 切换正弦轨迹参数名称
def traj_select(*args):
    if run_mode == 3:
        sel = combobox_traj.current()
        if sel == 1:
            label_parm1.configure(text='周期:')
        if sel == 0:
            label_parm1.configure(text='速度:')


# 自动筛选各个数据较好的两组数据
def find_bester():
    # 返回最小两个值的索引
    def find_min2(lis):
        def m_abs(val):
            if val < 0:
                return -val
            return val

        min_data = m_abs(lis[0])
        minindex0 = 0
        minindex1 = 1
        for n in range(1, len(lis)):
            if m_abs(lis[n]) <= min_data:
                min_data = m_abs(lis[n])
                minindex1 = minindex0
                minindex0 = n
            elif minindex0 == 0:
                if m_abs(lis[n]) < m_abs(lis[minindex1]):
                    minindex1 = n
        return [minindex0, minindex1]

    from openpyxl.styles import PatternFill
    import openpyxl
    filename = NNxlsx_entry.get()
    book = openpyxl.load_workbook(filename)
    sheet = book['原始数据']
    res = list(sheet.rows)
    rdata = []
    for i in range(1, sheet.max_row):
        rdata.append([i.value for i in res[i]][0:11] + [i + 1])
    # print(rdata)
    # print(rdata[0])
    # print([x[1] for x in rdata])
    index = []
    for i in range(8):
        index.append(find_min2([x[i] for x in rdata]))
    # print(index)

    if '较优数据' in book.sheetnames:
        book.remove(book['较优数据'])  # 清空表格
    book.create_sheet('较优数据')
    sh = book['较优数据']
    m_title = ['V_max', 'V_min', 'V_mean', 'V_var',
               'i_max', 'i_min', 'i_mean', 'i_var',
               'vel_gain', 'vel_inte', 'c_band', 'index']
    sh.append(m_title)  # 标题栏

    for i in range(8):
        sh.append([m_title[i]])
        # sh.merge_cells(range_string=f"A{3*i+2}:C{3*i+2}") #  合并单元格
        sh.cell(row=3 * i + 2, column=1).fill = PatternFill(fill_type='solid', fgColor='FFFF00')  # 设置单元格底色
        sh.append(rdata[index[i][0]])
        sh.append(rdata[index[i][1]])
    errorTimes = 0
    while True:
        try:
            if errorTimes == 3:
                MSG.showerror(f'错误', f'多次写入失败！')
                return False
            book.save(filename)
            MSG.showinfo('提示', '筛选已完成！')
            break
        except Exception:
            errorTimes = errorTimes + 1
            MSG.showerror(f'错误', f'文件{filename}写入失败！请关闭后再试')

def select_vel_gain():
    global vel_gain,vel_gain_initial
    vel_gain = get_text_data(text_vel_gain)
    text_vel_gain.delete(1.0, END)
    text_vel_gain.insert(END, '%0.3f' % vel_gain)
    vel_gain_initial = vel_gain

def Reset_parameters():
    global vel_inte
    vel_gain_initial = vel_gain
    text_vel_gain.delete(1.0,END)
    text_vel_gain.insert(END, '%0.3f' % vel_gain_initial)
    vel_inte = 10
    text_vel_inte.delete(1.0, END)
    text_vel_inte.insert(END, '%0.3f' % vel_inte)


# 以下内容为界面各控件布局设置
if True:
    frameL = Frame(root)
    frameL.place(x=0.0)
    frameL1 = Frame(root)
    frameL1.place(y=25)
    frameL2 = Frame(root)
    frameL2.place(y=50)
    frameL3 = Frame(root)
    frameL3.place(y=80)

    frameM = Frame(root)
    frameM.place(x=200)
    frameM1 = Frame(root)
    frameM1.place(x=200, y=25)
    frameM2 = Frame(root)
    frameM2.place(x=200, y=50)
    frameM3 = Frame(root)
    frameM3.place(x=200, y=80)

    frameR = Frame(root)
    frameR.place(x=360)

    frameD = Frame(root)
    frameD.place(y=130)
    frameDm = Frame(root)
    frameDm.place(y=165)
    frameD1 = Frame(root)
    frameD1.place(y=200)
    frameDR1 = Frame(root)
    frameDR1.place(x=360, y=130)
    frameDR = Frame(root)
    frameDR.place(x=360, y=200)
    frameD2 = Frame(root)
    frameD2.place(y=240)

    Label(frameL, text='COM:', width=6).pack(side=LEFT)
    combobox_coms = Combobox(frameL, values=COMS, width=17)
    combobox_coms.bind("<<ComboboxSelected>>", com_select)  # 绑定回调事件
    combobox_coms.pack(fill=X)

    Label(frameL1, text='波特率:', width=6).pack(side=LEFT)
    banudrate_entry = Entry(frameL1, width=7)
    banudrate_entry.pack(side=LEFT)
    Label(frameL1, text='ID:').pack(side=LEFT)
    id_entry = Entry(frameL1, width=8)
    id_entry.pack()
    button_ComOpen = Button(frameL2, text='连接CAN', command=button_ComOpen_command, width=13)
    button_ComOpen.pack(side=LEFT)
    button_usbOpen = Button(frameL2, text='连接USB', command=button_usbOpen_command, width=12)
    button_usbOpen.pack(fill=X)
    Button(frameL3, text='随机参数测试', command=lambda: buttono_randTset_command(1), width=13).pack(side=LEFT)
    Button(frameL3, text='自动测试', command=lambda: Automated_test(), width=12).pack(fill=X)

    label_parm1 = Label(frameM, text='速度:', width=4)
    label_parm1.pack(side=LEFT)
    speed_entry = Entry(frameM, width=15)
    speed_entry.pack(fill=X)
    Label(frameM1, text='角度:', width=4).pack(side=LEFT)
    pos_entry = Entry(frameM1, width=15)
    pos_entry.pack(fill=X)
    Label(frameM2, text='轨迹:', width=4).pack(side=LEFT)
    combobox_traj = Combobox(frameM2, values=['梯形轨迹', '正弦轨迹'], width=12)
    combobox_traj.pack(fill=X)
    combobox_traj.bind("<<ComboboxSelected>>", traj_select)  # 绑定回调事件

    button_startMotor = Button(frameM3, text='GUI', command=button_startMotor_command, width=9)
    button_startMotor.pack(side=LEFT)
    button_setZero = Button(frameM3, text='设置零点', command=button_setZero_command, width=9)
    button_setZero.pack(side=LEFT)

    var = IntVar()
    Radiobutton(frameR, text="扭矩模式", variable=var, value=1, command=mode_slect).pack()
    Radiobutton(frameR, text="速度模式", variable=var, value=2, command=mode_slect).pack()
    Radiobutton(frameR, text="角度模式", variable=var, value=3, command=mode_slect).pack()
    button_startTest = Button(frameR, text='运行测试', command=lambda: button_startTest_command(1), width=14)
    button_startTest.pack(fill=X)

    text_vel_gain = Text(frameDm, height=2, width=12)
    text_vel_gain.pack(side=LEFT)
    text_vel_inte = Text(frameDm, height=2, width=12)
    text_vel_inte.pack(side=LEFT)
    text_cur_band = Text(frameDm, height=2, width=12)
    text_cur_band.pack(side=LEFT)
    text_pos_gain = Text(frameDm, height=2, width=11)
    text_pos_gain.pack(side=LEFT)

    Button(frameD, text='增加速度增益', command=lambda: updata_vel_gain(1), width=12).pack(fill=X, side=LEFT)
    Button(frameD1, text='降低速度增益', command=lambda: updata_vel_gain(0), width=12).pack(fill=X, side=LEFT)
    Button(frameD, text='增加速度积分', command=lambda: updata_vel_inte(1)).pack(fill=X, side=LEFT)
    Button(frameD1, text='降低速度积分', command=lambda: updata_vel_inte(0)).pack(fill=X, side=LEFT)

    Button(frameD, text='增加电流带宽', command=lambda: updata_cur_band(1)).pack(fill=X, side=LEFT)
    Button(frameD1, text='降低电流带宽', command=lambda: updata_cur_band(0)).pack(fill=X, side=LEFT)
    Button(frameD, text='增加位置增益', command=lambda: updata_pos_gain(1)).pack(fill=X, side=LEFT)
    Button(frameD1, text='降低位置增益', command=lambda: updata_pos_gain(0)).pack(fill=X, side=LEFT)

    checkbutton_excel = Checkbutton(frameD2, text='自动保存 文件名称：', variable=excel_en, onvalue=1, offvalue=0)
    checkbutton_excel.pack(side=LEFT)
    excelname_entry = Entry(frameD2, width=15)
    excelname_entry.pack(side=LEFT)
    Label(frameD2, width=1).pack(side=LEFT)
    Button(frameD2, text='保存数据', width=10, command=button_save_command).pack(side=LEFT)
    Label(frameD2, width=2).pack(side=LEFT)
    Button(frameD2, text='保存运行模式', width=14, command=button_savemode_command).pack()

    Button(frameDR1, text='读取闭环参数', command=read_alldata, width=14).pack()
    Button(frameDR1, text='上传闭环参数', command=button_updata_command, width=14).pack()
    Label(frameDR, text='步长').pack(side=LEFT)
    step_entry = Entry(frameDR, width=8)
    step_entry.pack(side=LEFT)
    Label(frameDR, text='%').pack(side=LEFT)

    frameCur = Frame(root, width=320, height=120)
    frameCur.place(x=480)

    frameCurm = Frame(frameCur)
    frameCurm.place(y=0)
    Label(frameCurm, text='起始带宽:').pack(side=LEFT)
    curbandL_entry = Entry(frameCurm, width=12)
    curbandL_entry.pack(side=LEFT)

    frameCurm = Frame(frameCur)
    frameCurm.place(x=160, y=0)
    Label(frameCurm, text='终止带宽:').pack(side=LEFT)
    curbandH_entry = Entry(frameCurm, width=12)
    curbandH_entry.pack(side=LEFT)

    frameCurm = Frame(frameCur)
    frameCurm.place(y=25)
    Label(frameCurm, text='带宽步进:').pack(side=LEFT)
    curbandS_entry = Entry(frameCurm, width=12)
    curbandS_entry.pack(side=LEFT)

    frameCurm = Frame(frameCur)
    frameCurm.place(x=160, y=25)
    Label(frameCurm, text='最大扭矩:').pack(side=LEFT)
    cur_entry = Entry(frameCurm, width=12)
    cur_entry.pack(fill=X)

    frameCurm = Frame(frameCur)
    frameCurm.place(x=0, y=50)
    Label(frameCurm, text='文档名称:').pack(side=LEFT)
    xlsx_entry = Entry(frameCurm, width=35)
    xlsx_entry.pack(fill=X)

    frameCurm = Frame(frameCur)
    frameCurm.place(x=0, y=80)
    Button(frameCurm, text='启动带宽测试', command=tor_test, width=20).pack()

    frameCurm = Frame(frameCur)
    frameCurm.place(x=160, y=80)
    Button(frameCurm, text='测试最大扭矩', command=tor_test_max, width=20).pack()

    frameNNtest = Frame(root, width=320, height=240)
    frameNNtest.place(x=480, y=130)

    frameNNtestm = Frame(frameNNtest)
    frameNNtestm.place(x=0, y=0)
    NNpath_entry = Entry(frameNNtestm, width=32)
    Button(frameNNtestm, text='参数路径:', command=get_path).pack(side=LEFT)
    Label(frameNNtestm, text=' ').pack(side=LEFT)
    NNpath_entry.pack(side=LEFT)

    frameNNtestm = Frame(frameNNtest)
    frameNNtestm.place(y=30)
    Label(frameNNtestm, text='输入层数:    ').pack(side=LEFT)
    NNin_entry = Entry(frameNNtestm, width=10)
    NNin_entry.pack(fill=X)
    frameNNtestm = Frame(frameNNtest)
    frameNNtestm.place(x=160, y=30)
    Label(frameNNtestm, text='隐藏层数:').pack(side=LEFT)
    NNhid_entry = Entry(frameNNtestm, width=11)
    NNhid_entry.pack(fill=X)

    frameNNtestm = Frame(frameNNtest)
    frameNNtestm.place(y=60)
    Label(frameNNtestm, text='文档名称:    ').pack(side=LEFT)
    NNxlsx_entry = Entry(frameNNtestm, width=22)
    NNxlsx_entry.pack(side=LEFT)
    Label(frameNNtestm, text='').pack(side=LEFT)
    Button(frameNNtestm, text='自动筛选', command=find_bester, width=9).pack(fill=X)

    frameNNtestm = Frame(frameNNtest)
    frameNNtestm.place(y=90)
    Label(frameNNtestm, text='测试次数:    ').pack(side=LEFT)
    NNtimes_entry = Entry(frameNNtestm, width=10)
    NNtimes_entry.pack(fill=X)
    frameNNtestm = Frame(frameNNtest)
    frameNNtestm.place(x=164, y=89)
    Button(frameNNtestm, text='运行测试', command=start_NNtest, width=9).pack(side=LEFT)
    Button(frameNNtestm, text='随机赋参', command=lambda: buttono_randTset_command(0), width=9).pack(side=LEFT)

    frameNNtestm = Frame(frameNNtest)
    frameNNtestm.place(y=120)
    Label(frameNNtestm, text='测试信息:    ').pack(side=LEFT)
    NNmsg_entry = Entry(frameNNtestm, width=32)
    NNmsg_entry.pack(fill=X)

# 界面各项设置默认值
rand_gain = 2.0  # 随机增益最大值
rand_inte = 10  # 随机积分最大值

run_mode = 3  # 设置默认运行模式1：扭矩模式 2：速度模式 3：位置模式
var.set(run_mode)

combobox_coms.current(0)
banudrate_entry.insert(END, '115200')  # 默认串口波特率
id_entry.insert(END, '0')
step_entry.insert(END, '10')  # 默认步进值(%)
checkbutton_excel.select()  # 默认自动保存
excelname_entry.insert(END, '测试数据训练集.xlsx')  # 测试数据集文档名称

pos_entry.insert(END, '150')  # 默认角度值
combobox_traj.current(0)  # 位置模式运行轨迹 1：正弦轨迹模式 0：梯形轨迹模式
traj_select()
speed_entry.insert(END, '8')  # 速度/周期

curbandL_entry.insert(END, '100')  # 带宽测试起始带宽值
curbandH_entry.insert(END, '1000')  # 带宽测试终止带宽值
curbandS_entry.insert(END, '100')  # 带宽步进值
xlsx_entry.insert(END, '带宽测试数据.xlsx')  # 带宽测试文档名称

NNxlsx_entry.insert(END, '自动调参过程数据.xlsx')  # 自动调参过程数据文档名称
NNtimes_entry.insert(END, '50')  # 自动调参运行次数

style = ttk.Style()
style.theme_use('clam')
number = 10

# 生成一个顶级菜单实例
menubar = Menu(root)
# 生成一个子菜单实例
filemenu = Menu(menubar, tearoff=False)
accmenu = Menu(filemenu, tearoff=False)
# 子菜单中添加菜单项
filemenu.add_cascade(label="速度增益",menu=accmenu)
filemenu.add_cascade(label="重置参数",command=Reset_parameters)
# 绑定一个事件
accmenu.add_command(label='上传速度增益', command=select_vel_gain)
# 分割线
filemenu.add_separator()
filemenu.add_command(label="退出")
#为顶级菜单实例添加菜单，并级联相应的子菜单实例
menubar.add_cascade(label="菜单", menu=filemenu)#为filemenu命名
# 把菜单绘画在窗口上面
root.config(menu=menubar)

root.mainloop()  # 启动窗口
